package Controller;

import Interface.*;
import lejos.nxt.Button;
import lejos.nxt.ColorLightSensor;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.*;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;
import java.util.*;

/**
 * The robot will drive a straight line until the underground is the default
 * color. Then, it will decide if it has crossed a barcode, and which one.
 * Finally, it will give instructions to the pilot according o the detected
 * barcode.
 */
public class BarcodeControl {

	private static LightSensor sensor = new LightSensor(SensorPort.S1);
	private static TachoPilot pilot = new TachoPilot(2.20f, 6.30f, Motor.B,
			Motor.C);

	private static int marginLow = 25; // 20
	private static Hashtable barcodeMap;
	private static int marginHigh = 30;
	private static ArrayList<Integer> readings = new ArrayList<Integer>();
	private static ArrayList<Integer> colors = new ArrayList<Integer>();

	public static void main(String[] args) throws Exception {

		calibrateLightSensor();
		LCD.clear();
		LCD.drawString("Connect Bluetooth?", 0, 1);
		int button = Button.waitForPress();
		if (button == Button.ID_ENTER) {
			BluetoothNXT test = new BluetoothNXT(pilot, sensor,
					new UltrasonicSensor(SensorPort.S2));
			test.start();
		}

		barcodeMap = new Hashtable();
		setupBarcodes();

		action();
	}

	public static void calibrateLightSensor() throws InterruptedException {

		Thread.sleep(1000);
		System.out.println("Calibrating lightsensor");
		System.out.println("Set white value");
		Thread.sleep(1000);
		while (!Button.ENTER.isPressed()) {
			sensor.calibrateLow();
		}
		Thread.sleep(1000);
		System.out.println("Set black value");
		while (!Button.ENTER.isPressed()) {
			sensor.calibrateHigh();
		}
		System.out.println("Lightsensor calibrated");
		System.out.println("High: " + sensor.getHigh() + " Low: "
				+ sensor.getLow());
		System.out.println("Press enter to continue.");

		Button.ENTER.waitForPress();

	}

	public static void action() {

		while (true) {
			System.out.println("BarcodeReader");
			pilot.forward();
			// Motor.B.setSpeed(200);
			// Motor.C.setSpeed(200);
			/*
			 * try { testReadings(); } catch (InterruptedException e1) { }
			 */
			int counter = 0;

			while (counter < 3) {
				if (getColorFromValue(sensor.readValue()) != 2)
					counter++;

			}

			ArrayList<Integer> readings = getBarcodeReading();

			if (readings != null) {

				String detectedBarcode = calculateBarcode(readings);
				Integer barcodeNb = (Integer) barcodeMap.get(detectedBarcode);
				if (barcodeNb != null)
					System.out.println(barcodeNb);
				else {
					barcodeNb = (Integer) barcodeMap
							.get(invertBarcode(detectedBarcode));
					if (barcodeNb == null)
						System.out.println("null");
					else
						pilot.rotate(190);
				}
				try {
					Thread.sleep(500);
				} catch (InterruptedException e1) {
				}
				// Button.ENTER.waitForPress();
				executeBarcode(barcodeNb);
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {
				}
			}
			// Button.ENTER.waitForPress();
		}
	}

	/**
	 * Inverts the given string and returns the inversion
	 */
	private static Object invertBarcode(String detectedBarcode) {
		String ret = "";
		for (int i = detectedBarcode.length() - 1; i >= 0; i--) {
			ret += detectedBarcode.charAt(i);
		}
		return ret;
	}

	/**
	 * Returns whether the detected lightvalue is black (0), white(1) or
	 * neutral(2)
	 */
	private static int getColorFromValue(int value) {
		// Detected white
		if (value < 0 + marginLow)
			return 1;
		// Detected black
		if (value > 100 - marginHigh)
			return 0;
		else
			return 2;
	}

	/**
	 * Returns whether the detected lightvalue is black (0), white(1)
	 */
	private static int getColorFromValueBin(int value) {
		// Detected white
		if (value < 65)
			return 1;
		// Detected black
		else
			return 0;
	}

	/**
	 * Transforms the data from the lightsensor into strings representing a
	 * barcode. Returns null if the crossed underground wasn't a barcode (ie it
	 * could be a sideline).
	 * 
	 * @throws InterruptedException
	 */
	private static ArrayList<Integer> getBarcodeReading() {
		readings.clear();

		int currentReading = 0;
		int neutralValueCount = 0;

		while (neutralValueCount < 100) {
			currentReading = sensor.readValue();
			System.out.println(getColorFromValue(currentReading));

			if (getColorFromValue(currentReading) == 2)
				neutralValueCount++;
			else
				neutralValueCount = 0;

			readings.add(currentReading);

			if (readings.size() > 550) {
				System.out.println("Too long");
				return null;
			}

		}

		pilot.stop();

		// remove the last 100 (neutral) values from the readings
		for (int i = 0; i < 100; i++) {
			readings.remove(readings.size() - 1);
		}

		// prepare string with readings to print
		String output = "";
		for (int i = 0; i < readings.size(); i++) {
			output = output + " " + readings.get(i);
		}
		// System.out.println(output);
		System.out.println("readings.size= " + readings.size());

		if (readings.size() < 200) {
			System.out.println("Too short");
			return null;
		}
		return readings;
	}

	/**
	 * Returns a string which contains a barcode derived from given readings. It
	 * divides the readings in 7 equal blocks and calculates the mean of each
	 * block.
	 */
	private static String calculateBarcode(ArrayList<Integer> readings) {

		int barlength = Math.round(readings.size() / 7);
		colors.clear();

		for (int i = 0; i < 7; i++) {

			double tempsum = 0;
			for (int j = 0; j < barlength; j++) {
				tempsum += readings.get(i * barlength + j);
			}
			double meanValue = tempsum / barlength;

			colors.add(getColorFromValueBin((int) meanValue));
		}

		// prepare string to return
		String ret = "";
		for (int i = 0; i < colors.size(); i++) {
			int color = colors.get(i);
			ret = ret + color;
		}
		System.out.println("detected colors: ");
		System.out.println(ret);

		return ret;
	}

	/**
	 * Executes the pilot-commands according to given barcodenumber.
	 */
	private static void executeBarcode(Integer bcNb) {
		if (bcNb == null) {
			System.out.println("Invalid reading!");
			// recovery();
			bcNb = 100;
		}

		// TODO insert correct values
		float distance = 13f;
		float angle = 93;
		
		switch (bcNb) {

		case 1: // straight
			System.out.println("Straight!");
			pilot.travel(30f);
			break;
		case 3: // left turn
			System.out.println("Left!");
			pilot.travel(distance);
			pilot.rotate(angle);
			break;
		case 4: // up
			System.out.println("Up!");
			pilot.travel(30f);
			break;
		case 5: // down
			System.out.println("Down!");
			pilot.travel(30f);
			break;
		case 6: // right turn;
			System.out.println("Right!");
			pilot.travel(distance);
			pilot.rotate(-angle);
			break;
		case 100:
			pilot.backward();
			ArrayList<Integer> readings = getBarcodeReading();
			Integer barcodeNb = null;

			if (readings != null) {
				System.out.println("readings!=null");
				String detectedBarcode = calculateBarcode(readings);

				barcodeNb = (Integer) barcodeMap
						.get(invertBarcode(detectedBarcode));
				if (barcodeNb != null)
					System.out.println("recover: " + barcodeNb);
			}
			try {
				Thread.sleep(500);
			} catch (InterruptedException e1) {
			}
			if(barcodeNb==null){
				break;
			}
			executeBarcode(barcodeNb);
			try {
				Thread.sleep(500);
			} catch (InterruptedException e) {	}

			break;

		default:
			System.out.println("Detected bc doesnt respond to an action");
		}
	}

	/**
	 * Adds barcode-data to the barcodeMap See the online pdf for barcode
	 * numbers
	 */
	private static void setupBarcodes() {
		barcodeMap.put("0001111", 1); // straight
		barcodeMap.put("0110011", 6); // left turn
		barcodeMap.put("0011001", 3); // right turn
		barcodeMap.put("0010110", 4); // up
		barcodeMap.put("0100101", 5); // down

	}

	private static void testReadings() throws InterruptedException {

		System.out.println("executing testreadings");
		while (true) {

			int reading = sensor.readValue();
			System.out.println(getColorFromValue(reading) + " fake");
			System.out.println(reading + " real");

			Thread.sleep(3000);
		}
	}

}
